// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Dynamixel.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Robotis
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// Class for interacting with USB2Dynamixel controller
    /// </summary>
    public class Dynamixel : IDisposable
    {
        /// <summary>
        /// Default COM port for USB2Dynamixel
        /// </summary>
        public const int DefaultPortNumber = 3;

        /// <summary>
        /// Default Baud.  Indicates 1Mbps
        /// </summary>
        public const int DefaultBaudNumber = 1;

        /// <summary>
        /// Minimum position
        /// </summary>
        public const int MinimumPosition = 0;

        /// <summary>
        /// Maximum position
        /// </summary>
        public const int MaximumPosition = 1024;

        /// <summary>
        /// Minimum movement speed
        /// </summary>
        public const int MinimumSpeed = 0;

        /// <summary>
        /// Maximum movement speed
        /// </summary>
        public const int MaximumSpeed = 1024;

        /// <summary>
        /// Indicates if we are disposed or not.
        /// </summary>
        private bool disposed;

        /// <summary>
        /// Initializes a new instance of the <see cref="Dynamixel"/> class.
        /// </summary>
        /// <param name="portnumber">USB2Dynamixel COM port number.</param>
        /// <param name="baud">Baud rate for servos.</param>
        public Dynamixel(int portnumber = DefaultPortNumber, int baud = DefaultBaudNumber)
        {
            this.PortNumber = portnumber;
            this.Baud = baud;
            this.disposed = false;
            this.Connected = false;
        }

        /// <summary>
        /// Gets the port number.
        /// </summary>
        public int PortNumber { get; private set; }

        /// <summary>
        /// Gets the baud number
        /// </summary>
        public int Baud { get; private set; }

        /// <summary>
        /// Gets a value indicating whether a connection exists to the USB2Dynamixel
        /// </summary>
        public bool Connected { get; private set; }

        /// <summary>
        /// Open a connection
        /// </summary>
        public void Connect()
        {
            if (NativeMethods.dxl_initialize(this.PortNumber, this.Baud) == 0)
            {
                throw new ApplicationException(string.Format("Could not connect to USB2Dynamixel on COM{0} with Baud:  {1}", this.PortNumber, this.Baud));
            }

            this.Connected = true;
        }

        /// <summary>
        /// Set servo position
        /// </summary>
        /// <param name="id"> Servo ID.</param>
        /// <param name="position">Desired position.  Range: 0 - 1024. </param>
        /// <returns>True if command succeeded, false otherwise.</returns>
        public bool SetPosition(int id, int position)
        {
            if (!this.Connected)
            {
                TraceOut.Error(TraceContexts.HW, "Attempt to set servo position with USB2Dynamixel disconnected");
                return false;
            }

            if (position < MinimumPosition && position > MaximumPosition)
            {
                throw new ArgumentException(string.Format("Position out of range [{0}, {1}]:  {2}", MinimumPosition, MaximumPosition, position));
            }

            NativeMethods.dxl_write_word(id, NativeMethods.P_GOAL_POSITION_L, position);

            int status = NativeMethods.dxl_get_result();

            if (status == NativeMethods.COMM_RXSUCCESS)
            {
                return true;
            }

            TraceOut.Error(TraceContexts.HW, "Error transmitting to USB2Dynamixel.  Error code:  {0}.  Servo ID: {1}", status, id);

            return false;
        }

        /// <summary>
        /// Read servo position
        /// </summary>
        /// <param name="id">Servo id.</param>
        /// <returns>Servo position. -1 on error.</returns>
        public int GetPosition(int id)
        {
            if (!this.Connected)
            {
                TraceOut.Error(TraceContexts.HW, "Attempt to read servo position with USB2Dynamixel disconnected");
                return -1;
            }

            int position = NativeMethods.dxl_read_word(id, NativeMethods.P_PRESENT_POSITION_L);

            int status = NativeMethods.dxl_get_result();

            if (status != NativeMethods.COMM_RXSUCCESS)
            {
                TraceOut.Error(TraceContexts.HW, "Error reading servo position.  Error code:  {0}.  Servo ID: {1}", status, id);
                return -1;
            }

            return position;
        }

        /// <summary>
        /// Set servo position
        /// </summary>
        /// <param name="id"> Servo ID.</param>
        /// <param name="speed"> Movement speed. Range: 0 - 1024.  0 indicates no limit (as fast as possible). </param>
        /// <returns>True if command succeeded, false otherwise.</returns>
        public bool SetMoveSpeed(int id, int speed)
        {
            if (!this.Connected)
            {
                TraceOut.Error(TraceContexts.HW, "Attempt to set move speed with USB2Dynamixel disconnected");
                return false;
            }

            if (speed < MinimumSpeed && speed > MaximumSpeed)
            {
                throw new ArgumentException(string.Format("Speed out of range [{0}, {1}]:  {2}", MinimumSpeed, MaximumSpeed, speed));
            }

            NativeMethods.dxl_write_word(id, NativeMethods.P_MOVE_SPEED_L, speed);

            int status = NativeMethods.dxl_get_result();

            if (status == NativeMethods.COMM_RXSUCCESS)
            {
                return true;
            }

            TraceOut.Error(TraceContexts.HW, "Error transmitting to USB2Dynamixel.  Error code:  {0}.  Servo ID: {1}", status, id);

            return false;
        }

        /// <summary>
        /// Dispose of resources (close connection).
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Dispose of resources
        /// </summary>
        /// <param name="disposing">True if Dispose() was called.</param>
        private void Dispose(bool disposing)
        {
            if (!this.disposed)
            {
                if (disposing)
                {
                    // dispose of managed resources
                    // we don't have any
                }

                NativeMethods.dxl_terminate();

                this.disposed = true;
                this.Connected = false;
            }
        }
    }
}
